package com.yuneec.droneservice.command;

import android.util.Log;

import com.MAVLink.yuneec.msg_mav_rc_cmd_ack;
import com.yuneec.droneservice.model.JoystickValues;
import com.yuneec.droneservice.request.IResponse;

/**
 * Created by gjw on 2017/3/27.
 */

public class GetJoystickValue extends BaseCommand {

    public GetJoystickValue() {
        super(MAV_RC_COMMAND.MAV_RC_CMD_GET_HW_INPUT_VAL);
    }

    @Override
    protected short[] body() {
        return null;
    }

    @Override
    public IResponse<JoystickValues> toResponse(msg_mav_rc_cmd_ack ack) {
        int code = ack.code;
        IResponse<JoystickValues> response = new IResponse(ids(), code);
        if (code == MAV_RC_ERROR_CODE.MAV_RC_ERR_SUCCESS) {
            try {
                JoystickValues joystickValues = new JoystickValues();
                response.setParam(joystickValues);
                int index = 0;
                int[] jvalues = new int[ack.params[index++] & 0xFF];
                joystickValues.setjVaules(jvalues);
                int[] kvalues = new int[ack.params[index++] & 0xFF];
                joystickValues.setkVaules(kvalues);
                int[] svalues = new int[ack.params[index++] & 0xFF];
                joystickValues.setsVaules(svalues);
                int[] bvalues = new int[ack.params[index++] & 0xFF];
                joystickValues.setbVaules(bvalues);
                for (int i = 0; i < jvalues.length; i++) {
                    jvalues[i] = uIntArrayToShort(ack.params, index);
                    index += 2;
                }
                for (int i = 0; i < kvalues.length; i++) {
                    kvalues[i] = uIntArrayToShort(ack.params, index);
                    index += 2;
                }
                for (int i = 0; i < svalues.length; i++) {
                    svalues[i] = uIntArrayToShort(ack.params, index);
                    index += 2;
                }
                for (int i = 0; i < bvalues.length; i++) {
                    bvalues[i] = uIntArrayToShort(ack.params, index);
                    index += 2;
                }
            } catch (Exception e) {
                Log.e("zzd_aoa", "getJoystick:" + e.getMessage());
                response.setCode(MAV_RC_ERROR_CODE.MAV_RC_ERR_UNKNOW);
            }
        }
        return response;
    }

}
